import numpy as np
from scipy.interpolate import RBFInterpolator

# 预处理校准数据
calibration_data = [
    {"center": [120, 120], "valid": True, "offset": [-5.25, 8.25]},
    {"center": [120, 200], "valid": True, "offset": [-1.75, 10.0]},
    {"center": [120, 280], "valid": True, "offset": [-2.25, 9.5]},
    {"center": [120, 360], "valid": True, "offset": [-6.0, 12.25]},
    {"center": [200, 40], "valid": True, "offset": [0.75, 11.5]},
    {"center": [200, 120], "valid": True, "offset": [5.75, 11.5]},
    {"center": [200, 200], "valid": True, "offset": [7.5, 9.5]},
    {"center": [200, 280], "valid": True, "offset": [9.0, 9.0]},
    {"center": [200, 360], "valid": True, "offset": [7.75, 11.0]},
    {"center": [200, 440], "valid": True, "offset": [6.5, 13.75]},
    {"center": [280, 40], "valid": True, "offset": [7.25, 9.75]},
    {"center": [280, 120], "valid": True, "offset": [12.5, 6.5]},
    {"center": [280, 200], "valid": True, "offset": [14.25, 10.5]},
    {"center": [280, 280], "valid": True, "offset": [17.0, 8.0]},
    {"center": [280, 360], "valid": True, "offset": [16.0, 6.5]},
    {"center": [280, 440], "valid": True, "offset": [15.5, 7.75]},
    {"center": [360, 40], "valid": True, "offset": [11.0, 10.0]},
    {"center": [360, 120], "valid": True, "offset": [16.5, 8.25]},
    {"center": [360, 200], "valid": True, "offset": [19.0, 6.75]},
    {"center": [360, 280], "valid": True, "offset": [20.75, 1.75]},
    {"center": [360, 360], "valid": True, "offset": [19.0, 0.25]},
    {"center": [360, 440], "valid": True, "offset": [17.75, 3.0]},
    {"center": [440, 120], "valid": True, "offset": [12.25, 11.0]},
    {"center": [440, 200], "valid": True, "offset": [-6.5, -1.75]},
    {"center": [440, 280], "valid": True, "offset": [6.25, 4.5]},
    {"center": [440, 360], "valid": True, "offset": [20.75, -4.75]},
    {"center": [440, 440], "valid": True, "offset": [18.75, 0.5]},
]


# 提取有效数据点
points = []
offsets_x = []
offsets_y = []
USE_CALI = True

for item in calibration_data:
    if item["valid"]:
        x, y = item["center"]
        offset = item["offset"]
        points.append([x, y])
        offsets_x.append(offset[0])
        offsets_y.append(offset[1])

points = np.array(points)
offsets_x = np.array(offsets_x)
offsets_y = np.array(offsets_y)

# 创建RBF插值器
rbf_offset_x = RBFInterpolator(points, offsets_x, kernel="thin_plate_spline")
rbf_offset_y = RBFInterpolator(points, offsets_y, kernel="thin_plate_spline")


def calculate_target_position(desired_position, max_iterations=20, tolerance=0.1):
    # DEBUG ONLY
    # return desired_position
    """
    计算需要发送给机械臂的坐标以实现目标摄像机坐标
    参数:
        desired_position (list/tuple): 期望的摄像机坐标 [x, y]
        max_iterations (int): 最大迭代次数
        tolerance (float): 收敛阈值（像素）
    返回:
        list: 应发送给机械臂的坐标 [x, y]
    """
    desired = np.array(desired_position, dtype=np.float64)
    current_guess = desired.copy()

    for _ in range(max_iterations):
        # 预测当前猜测位置的偏移量
        offset_x = rbf_offset_x(current_guess.reshape(1, -1))[0]
        offset_y = rbf_offset_y(current_guess.reshape(1, -1))[0]

        # 计算新的猜测位置
        new_guess = desired - np.array([offset_x, offset_y])

        # 检查收敛
        if np.linalg.norm(new_guess - current_guess) < tolerance:
            # print("Converged!: ", new_guess)
            return [round(float(new_guess[0]), 1), round(float(new_guess[1]), 1)]

        current_guess = new_guess

    print("Result!: ", current_guess)
    return [round(float(current_guess[0]), 1), round(float(current_guess[1]), 1)]


# 使用示例
if __name__ == "__main__":
    # 测试用例：已知校准点对应的期望位置
    test_desired = [237, 296]  # 应返回 [200, 200]
    result = calculate_target_position(test_desired)
    print(f"目标坐标 {test_desired} 对应的机械臂坐标为: {result}")
